#include "camera.h"
#include "common_log.h"
#include <string>
#include <vector>
#include <thread>
#include <chrono>
#include <fcntl.h>
#include <cstring>
#include <iostream>
#include <dirent.h>
#include <unistd.h>
#include <algorithm>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <linux/videodev2.h>


namespace rc {

    const int V4LCamera::MAX_RECONNECT_ATTEMPTS = 5;

    /***
     * @description: check device state, isConnect or isStop
     *
     * @param {char*} device_path
     * @return {*}
     */
    bool isDeviceInUse(const char* device_path) {
        std::string command = "fuser " + std::string(device_path) + " 2>&1";
        FILE* pipe = popen(command.c_str(), "r");
        if (!pipe) return false;

        char buffer[128];
        std::string result = "";
        while(!feof(pipe)) {
            if(fgets(buffer, 128, pipe) != NULL)
                result += buffer;
        }
        pclose(pipe);

        return !result.empty();
    }


    /***
     * @description: Special case, process close faile state.
     *
     * @param {char*} device_path
     * @return {*}
     */
    bool forceReleaseDevice(const char* device_path) {
        std::string check_cmd = "fuser " + std::string(device_path) + " 2>/dev/null";
        int ret = system(check_cmd.c_str());

        if (ret == 0) {
            std::string ps_cmd = "fuser -v " + std::string(device_path) + " 2>&1";
            FILE* pipe = popen(ps_cmd.c_str(), "r");
            if (pipe) {
                pclose(pipe);
            }

            std::string kill_cmd = "fuser -k " + std::string(device_path) + " 2>/dev/null";
            system(kill_cmd.c_str());

            std::this_thread::sleep_for(std::chrono::milliseconds(500));

            ret = system(check_cmd.c_str());
            return (ret != 0);
        }

        return true;
    }


    /***
     * @description:
     * @param {char*} device_path
     * @param {int} max_attempts
     * @return {*}
     */
    bool tryOpenDevice(const char* device_path, int max_attempts = 3) {
        for (int i = 0; i < max_attempts; i++) {
            int fd = open(device_path, O_RDWR);
            if (fd >= 0) {
                close(fd);
                return true;
            }

            if (errno == EBUSY) {
                if (forceReleaseDevice(device_path)) {
                    continue;
                }
            }

            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        }

        return false;
    }


    /***
     * @description:
     * @param {char*} device_path
     * @return {*}
     */
    bool resetDevice(const char* device_path) {
        RCLogger& logger = RCLogger::getInstance();

        int fd = open(device_path, O_RDWR);
        if (fd == -1) {
            logger.rs_error(" [Error] (resetDevice) Failed to open device: {}", device_path);
            return false;
        }

        v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        bool streaming = false;
        if (ioctl(fd, VIDIOC_G_INPUT, &streaming) >= 0) {
            if (streaming) {
                logger.rs_info(" [Info] (resetDevice) Device is streaming, trying to stop...");
                if (ioctl(fd, VIDIOC_STREAMOFF, &type) == -1) {
                    logger.rs_error(" [Error] (resetDevice) Failed to stop stream: {}", strerror(errno));
                    throw std::runtime_error(" [Error] Failed to stop steaming! Please unplug and replug the USB module, wait for 5 seconds, then restart.");
                }
            }
        }

        struct v4l2_requestbuffers req = {};
        req.count = 0;
        req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        req.memory = V4L2_MEMORY_MMAP;
        if (ioctl(fd, VIDIOC_REQBUFS, &req) == -1) {
            if (errno != EINVAL) {
                logger.rs_error(" [Error] (resetDevice) Failed to release buffers: {}", strerror(errno));
                throw std::runtime_error(" [Error] Failed to release buffers! Please unplug and replug the USB module, wait for 5 seconds, then restart.");
            }
        }

        close(fd);
        std::this_thread::sleep_for(std::chrono::seconds(3));
        return true;
    }


    /***
     * @description: get all video node.
     *
     * @return {*}
     */
    std::vector<int> get_all_video_ids() {
        DIR *dir;
        struct dirent *entry;
        std::vector<int> video_ids;

        dir = opendir("/dev");
        if (dir == nullptr) {
            return video_ids;
        }

        while ((entry = readdir(dir)) != nullptr) {
            if (strncmp(entry->d_name, "video", 5) == 0 && strlen(entry->d_name) > 5) {
                int id = atoi(entry->d_name + 5);
                video_ids.push_back(id);
            }
        }

        closedir(dir);
        std::sort(video_ids.begin(), video_ids.end());
        return video_ids;
    }


    bool V4LCamera::findV4l2CameraDevicePath() {
        // Initial logger
        RCLogger& logger = RCLogger::getInstance();

        std::vector<int> video_indexs = get_all_video_ids();
        int deviceCount = video_indexs.size();
        if (deviceCount > 0) {
            logger.rs_info(" [Info] (findCameraDevice) Found valid video deices count: {}", deviceCount);
        } else {
            logger.rs_error(" [Error] (findCameraDevice) No video devices found.");
            return false;
        }

        for (int id : video_indexs) {
            std::ostringstream devPathStream;
            devPathStream << "/dev/video" << id;
            std::string devPath = devPathStream.str();

            int fd = open(devPath.c_str(), O_RDWR);
            if (fd == -1) continue;  // Can not open this node, skip

            struct v4l2_capability cap;
            if (ioctl(fd, VIDIOC_QUERYCAP, &cap) == 0) {
                std::string dev_name(reinterpret_cast<char*>(cap.card));

                if (dev_name.find(mCamCardName) != std::string::npos) {
                    logger.rs_info(" [Info] (findCameraDevice) Success find camera device({}) path: {}, fd: {}",
                                   mCamCardName,
                                   devPath,
                                   fd);

                    mCamConfig.devicePath = devPath;
                    mCamFd = fd;

                    return true;
                }
            }
            close(fd);
        }

        return false;
    }


    /***
     * @description: initial config: get param, initial.
     *
     * @param {int} width
     * @param {int} height
     * @param {uint32_t} pixelFmt
     * @param {int} fps
     * @return {*}
     */
    bool V4LCamera::initialize(int width, int height, uint32_t pixelFmt, int fps) {
        // Initial logger
        RCLogger& logger = RCLogger::getInstance();

        bool state = findV4l2CameraDevicePath();
        if (!state) {
            logger.rs_error(" [Error] (initialize) Failed to find device. cam({}).", mCamCardName);
        }

        logger.rs_info(" [Info] (initialize) Successed to find device, cam({}), path: [{}].",
                       mCamCardName,
                       mCamConfig.devicePath);

        mCamConfig.width = width;
        mCamConfig.height = height;
        mCamConfig.pixelFormat = pixelFmt;
        mCamConfig.fps = fps;

        if (!resetDevice(mCamConfig.devicePath.c_str())) {
            logger.rs_error(" [Error] (initialize) Failed to reset device: {}", mCamConfig.devicePath);
            throw std::runtime_error(" [Error] Failed to reset device! Please unplug and replug the USB module, wait for 5 seconds, then restart.");
            return false;
        }

        if (!tryOpenDevice(mCamConfig.devicePath.c_str(), 2)) {
            logger.rs_error(" [Error] (initialize) Failed to open device after attempts, cam: {}, path: {}",
                           mCamCardName, mCamConfig.devicePath);
            throw std::runtime_error(" [Error] Failed to open device(Try 2 times)! Please unplug and replug the USB module, wait for 5 seconds, then restart.");
            return false;
        }

        state = openDevice();
        if (!state) {
            logger.rs_error(" [Error] (initialize) Failed to open device, cam({}), fd({}), path: [{}], err: [{}].",
                            mCamCardName,
                            mCamFd,
                            mCamConfig.devicePath,
                            strerror(errno));
            return false;
        }

        bool fmtState = setFormat();
        bool frameRateState = setFrameRate();
        bool mmapInitState = initMmap();

        if (!fmtState || !frameRateState || !mmapInitState) {
            close(mCamFd);
            mCamFd = -1;
            logger.rs_error(" [Error] (initialize) Failed init cam({}), path: [{}], State: setFormat({}), setFrameRate({}), initMmap({}).",
                            mCamCardName,
                            mCamConfig.devicePath,
                            fmtState,
                            frameRateState,
                            mmapInitState);
            return false;
        }

        logger.rs_info(" [Info] (initialize) Successed to initial device({}), path: [{}].",
                       mCamCardName,
                       mCamConfig.devicePath);

        return true;
    }


    /***
     * @description:
     *
     * @return {*}
     */
    bool V4LCamera::setFormat() {
        // Initial logger
        RCLogger& logger = RCLogger::getInstance();

        v4l2_format fmt = {};
        fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        fmt.fmt.pix.width = mCamConfig.width;
        fmt.fmt.pix.height = mCamConfig.height;
        fmt.fmt.pix.pixelformat = mCamConfig.pixelFormat;
        fmt.fmt.pix.field = V4L2_FIELD_NONE;

        if (ioctl(mCamFd, VIDIOC_S_FMT, &fmt) == -1) {
            logger.rs_error(" [Error] (setFormat) Failed to set cam({}), format error({}) ",
                            mCamCardName,
                            strerror(errno));
            return false;
        }

        return true;
    }


    /***
     * @description:
     *
     * @return {*}
     */
    bool V4LCamera::setFrameRate() {
        // Initial logger
        RCLogger& logger = RCLogger::getInstance();

        v4l2_streamparm parm = {};
        parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        parm.parm.capture.timeperframe.numerator = 1;
        parm.parm.capture.timeperframe.denominator = mCamConfig.fps;

        if (ioctl(mCamFd, VIDIOC_S_PARM, &parm) == -1) {
            logger.rs_error(" [Error] (setFrameRate) Failed to set cam({}), framerate error({}).",
                            mCamCardName,
                            strerror(errno));
            return false;
        }

        return true;
    }


    /***
     * @description:
     *
     * @return {*}
     */
    bool V4LCamera::initMmap() {
        // Initial logger
        RCLogger& logger = RCLogger::getInstance();

        v4l2_requestbuffers req = {};
        req.count = 4;
        req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        req.memory = V4L2_MEMORY_MMAP;

        if (ioctl(mCamFd, VIDIOC_REQBUFS, &req) == -1) {
            logger.rs_error(" [Error] (initMmap) Failed to request cam({}) buffer, error({})",
                            mCamCardName,
                            strerror(errno));
            return false;
        }

        mBuffers.resize(req.count);

        for (unsigned int i = 0; i < req.count; ++i) {
            v4l2_buffer buf = {};
            buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
            buf.memory = V4L2_MEMORY_MMAP;
            buf.index = i;

            if (ioctl(mCamFd, VIDIOC_QUERYBUF, &buf) == -1) {
                logger.rs_error(" [Error] (initMmap) Failed to query cam({}) buffer, error: [{}]",
                                mCamCardName,
                                strerror(errno));
                return false;
            }

            mBuffers[i].length = buf.length;
            mBuffers[i].start = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, mCamFd, buf.m.offset);

            if (mBuffers[i].start == MAP_FAILED) {
                logger.rs_error(" [Error] (initMmap) Failed to mmap cam({}) buffer, error: [{}].",
                                mCamCardName,
                                strerror(errno));
                return false;
            }
        }

        return true;
    }


    /***
     * @description:
     *
     * @return {*}
     */
    bool V4LCamera::startStreaming() {
        // Initial logger
        RCLogger& logger = RCLogger::getInstance();

        for (unsigned int i = 0; i < mBuffers.size(); ++i) {
            v4l2_buffer buf = {};
            buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
            buf.memory = V4L2_MEMORY_MMAP;
            buf.index = i;

            if (ioctl(mCamFd, VIDIOC_QBUF, &buf) == -1) {
                logger.rs_error(" [Error] (startStreaming) Failed to queue cam({}) buffer, error: [{}].",
                                mCamCardName,
                                strerror(errno));
                return false;
            }
        }

        v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        if (ioctl(mCamFd, VIDIOC_STREAMON, &type) == -1) {
            logger.rs_error(" [Error] (startStreaming) Failed to start cam({}) streaming, error: [{}]",
                            mCamCardName);
            return false;
        }

        mIsStreaming = true;
        logger.rs_info(" [Info] Success start streaming, cam({}), path: [{}]",
                       mCamCardName,
                       mCamConfig.devicePath);

        return true;
    }


    /***
     * @description:
     *
     * @return {*}
     */
    bool V4LCamera::captureFrame(std::vector<uint8_t>& frameData) {
        // Initial logger
        RCLogger& logger = RCLogger::getInstance();

        if (!checkDeviceStatus()) {
            if (!reconnect()) {
                logger.rs_warn(" [WARN] Trigger reconnect...");
                return false;
            }
        }

        fd_set fds;
        FD_ZERO(&fds);
        FD_SET(mCamFd, &fds);

        struct timeval tv = {};
        tv.tv_sec = 2;

        int r = select(mCamFd + 1, &fds, NULL, NULL, &tv);
        if (r == -1) {
            logger.rs_error(" [Error] (captureFrame) cam({}), Select error: [{}]",
                            mCamCardName,
                            strerror(errno));
            return false;
        }

        if (r == 0) {
            logger.rs_error(" [Error] (captureFrame) cam({}), Select timeout.", mCamCardName);
            return false;
        }

        v4l2_buffer buf = {};
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;

        if (ioctl(mCamFd, VIDIOC_DQBUF, &buf) == -1) {
            logger.rs_error(" [Error] (captureFrame) cam({}), Failed to dequeue buffer: [{}]",
                            mCamCardName,
                            strerror(errno));
            return false;
        }

        frameData.assign((uint8_t*)mBuffers[buf.index].start, (uint8_t*)mBuffers[buf.index].start + buf.bytesused);

        if (ioctl(mCamFd, VIDIOC_QBUF, &buf) == -1) {
            logger.rs_error(" [Error] (captureFrame) cam({}), Failed to requeue buffer: [{}]",
                            mCamCardName,
                            strerror(errno));
            return false;
        }

        return true;
    }

    /***
     * @description:
     *
     * @return {*}
     */
    bool V4LCamera::stopStreaming() {
        // Initial logger
        RCLogger& logger = RCLogger::getInstance();

        if (mIsStreaming) {
            v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
            if (ioctl(mCamFd, VIDIOC_STREAMOFF, &type) == -1) {
                logger.rs_error(" [Error] (stopStreaming) Failed to stop cam({}) streaming: [{}]",
                                mCamCardName,
                                strerror(errno));
                return false;
            }

            mIsStreaming = false;
        } else {
            logger.rs_info( " [Info] current streaming already stop!");
        }
        return true;
    }


    /***
     * @description:
     *
     * @return {*}
     */
    void V4LCamera::closeDevice() {
        // Initial logger
        RCLogger& logger = RCLogger::getInstance();

        stopStreaming();

        for (auto& buffer : mBuffers) {
            munmap(buffer.start, buffer.length);
            buffer.start = nullptr;
            buffer.length = 0;
        }

        mBuffers.clear();

        if (mCamFd != -1) {
            close(mCamFd);
            mCamFd = -1;
        }

        if (isDeviceInUse(mCamConfig.devicePath.c_str())) {
            logger.rs_warn(" [WARN] Force closing device, please waitting...");
            forceReleaseDevice(mCamConfig.devicePath.c_str());
        }

        mIsStreaming = false;
    }


    /***
     * @description:
     *
     * @return {*}
     */
    bool V4LCamera::openDevice() {
        // Initial logger
        RCLogger& logger = RCLogger::getInstance();

        mCamFd = open(mCamConfig.devicePath.c_str(), O_RDWR);
        if (mCamFd == -1) {
            logger.rs_error(" [Error] (openDeivce) Failed to open device, cam({}), fd({}), path({}).",
                            mCamCardName,
                            mCamFd,
                            mCamConfig.devicePath);
            return false;
        }

        return true;
    }


    /***
     * @description:
     *
     * @return {*}
     */
    bool V4LCamera::checkDeviceStatus() {
        // Initial logger
        RCLogger& logger = RCLogger::getInstance();

        if (mCamFd == -1) return false;

        struct v4l2_capability cap;
        if (ioctl(mCamFd, VIDIOC_QUERYCAP, &cap) == -1) {
            logger.rs_error(" [Error] (checkDeviceStatus) Failed to get capability, cam({}), fd({}), path({})",
                            mCamCardName,
                            mCamFd,
                            mCamConfig.devicePath);
            return false;
        }

        return true;
    }


    /***
     * @description:
     *
     * @return {*}
     */
    bool V4LCamera::reconnect() {
        // Initial logger
        RCLogger& logger = RCLogger::getInstance();

        if (mReconnectAttempts >= MAX_RECONNECT_ATTEMPTS) {
            logger.rs_error(" [Error] (reconnect) Cam({}): Reconnect times > ({}), exit!",
                            mCamCardName,
                            MAX_RECONNECT_ATTEMPTS);
            return false;
        }

        resetDevice(mCamConfig.devicePath.c_str());

        if (initialize(mCamConfig.width, mCamConfig.height, mCamConfig.pixelFormat, mCamConfig.fps)) {
            logger.rs_info(" [Info] (reconnect) Cam({}): Successfully reconnected.", mCamCardName);
            mReconnectAttempts = 0;
            return true;
        }

        logger.rs_error(" [Error] (reconnect) Cam({}), Reconnect times: ({}), Continue reconnect!",
                        mCamCardName,
                        mReconnectAttempts);

        mReconnectAttempts++;
        return false;
    }
}

